#pragma once
#include "SFML\Graphics.hpp"
#include <iostream>
#include <vector>

class BlockField
{
	std::vector<sf::Vector2u> _blocked_point;

public:

	void AddPoint(sf::Vector2u p)
	{
		_blocked_point.push_back(p);
	}
	bool IsPointBlocked(sf::Vector2u p)
	{
		//std::cout << "4" <<std::endl;
		int size = _blocked_point.size();
		for (int i = 0; i < size; i++)	//for each blocked point
		{
			if ( _blocked_point.at(i).x == p.x &&  _blocked_point.at(i).y == p.y)
			{
				//std::cout << "5" <<std::endl;
				return true;
			}
		}
		//std::cout << "5" <<std::endl;
		return false;
	}
};

class Vertex
{
	bool _blocked;
	int _dist_from_point;
	Vertex* _next_to[4];
	bool _next_to_blocked[4];
	//std::vector <float> _distance_to;
	sf::Vector2u _pos;

public:

	Vertex(sf::Vector2u p)
	{
		_dist_from_point = 0;
		_pos = p;
		_blocked = false;
		for (int i = 0; i < 4; i++){_next_to_blocked[i] = false;}
	}

	sf::Vector2u get_position(){return _pos;}
	//int get_next_to_size(){return -1;}
	int get_distance_from_point(){return _dist_from_point;}
	void set_blocked(){_blocked = true; _dist_from_point = -1;}
	void set_next_to_blocked(int i){_next_to_blocked[i]= true;}
	bool is_blocked(){return _blocked;}
	void AddPoint(Vertex * v, int index)
	{
		_next_to[index] = v;
		//_distance_to.push_back( (_pos.x - v._pos.x)*(_pos.x - v._pos.x) + (_pos.y - v._pos.y)*(_pos.y - v._pos.y));
	}
	void get_tile_distance_from_point_recursive(bool been_to[][16], int current_dist, int * count_number, int max_size)
	{
		(*count_number) ++;
		if( (*count_number) < max_size)
		{
			_dist_from_point = current_dist + 1;
			//std::cout << " dist = "  << current_dist + 1<<" count = " << *count_number << std::endl;
			been_to[_pos.x][_pos.y] = true;

			for (int i = 0; i < 4; i++)
			{
				if(!_next_to_blocked[i] )
				{
					if (_next_to[i]!= NULL)
					{
						sf::Vector2u next_pos = _next_to[i]->get_position();
						if (!been_to[next_pos.x][next_pos.y] == true)
						{
							//std::cout << i << " is next to me" <<std::endl;
							_next_to[i]->get_tile_distance_from_point_recursive(been_to,current_dist + 1,count_number,max_size);
						}
					}
					else
					{
						//std::cout << i << " null" <<std::endl;
					}
				}
				else
				{
					//std::cout << i << " blocked" <<std::endl;
				}
			}
		}
		else
		{
			//std::cout << "MaxSize done" <<std::endl;
		}
		//std::cout << "asdf" <<std::endl;
	}
};

class a_star
{

	class node
	{
	public:
		sf::Vector2u _pos;	
		node (sf::Vector2u pos){_pos = pos;}
	};
public:
	int find_cost(sf::Vector2u a, sf::Vector2u b)
	{
		int dx = a.x - b.x;	if (dx < 0){dx = -dx;}
		int dy = a.y - b.y;	if (dy < 0){dy = -dy;}
		return dx+dy;
	}
	void find_path (sf::Vector2u start, sf::Vector2u end, std::vector<sf::Vector2u> blocks)
	{
		std::vector<node> closed_set;
		std::vector<node> open_set;		open_set.push_back(node(start));
		std::vector<node> came_from;
		int g_score[50];
		int f_score[50];


		while(open_set.size() > 0)
		{
			int low_f_score = 999999;
			for (int i  =0; i < open_set.size(); i++)
			{
				if (open_set.
			}
		}

	}
};

class VectorField
{
public :
	int _vector[16][16];
	int _max;

	sf::Vector2u getNextPoint(sf::Vector2u p)
	{
		sf::Vector2u nP;
		int lowVal = 9999;
		if (p.x-1 >= 0 && _vector[p.x-1][p.y] < lowVal){lowVal = _vector[p.x-1][p.y]; nP = sf::Vector2u(p.x-1,p.y);}
		if (p.x+1 <= 15 && _vector[p.x+1][p.y] < lowVal){lowVal = _vector[p.x+1][p.y]; nP = sf::Vector2u(p.x+1,p.y);}
		if (p.y-1 >= 0 &&_vector[p.x][p.y-1] < lowVal){lowVal = _vector[p.x][p.y-1]; nP = sf::Vector2u(p.x,p.y-1);}
		if (p.y-1 <= 15 &&_vector[p.x][p.y+1] < lowVal){lowVal = _vector[p.x][p.y+1]; nP = sf::Vector2u(p.x,p.y+1);}
		return nP;
	}

	int calc_max_value()
	{
		int m= -1;
		for (int i =  0; i < 16; i++)
		{
			for (int j = 0; j < 16; j++)
			{
				if (_vector[i][j] > m){m = _vector[i][j];}
			}
		}
		_max = m;
		return m;
	}

	void drawRoute(sf::RenderWindow & w,std::vector<sf::Vector2u> pts)
	{
		sf::RectangleShape _rect;
		_rect = sf::RectangleShape(sf::Vector2f(16,16));
		_rect.setPosition(sf::Vector2f(0,0));
		_rect.setFillColor(sf::Color(144,33,223));

		for each(sf::Vector2u p in pts)
		{
			_rect.setPosition(sf::Vector2f(p.x*16,p.y*16));
			_rect.setFillColor(sf::Color(255,0,0));
			w.draw(_rect);
		}
	}

	void draw(sf::RenderWindow & w)
	{
		sf::RectangleShape _rect;
		_rect = sf::RectangleShape(sf::Vector2f(16,16));
		_rect.setPosition(sf::Vector2f(0,0));
		_rect.setFillColor(sf::Color(144,33,223));

		for (int i =0; i < 16; i++)
		{
			for (int j = 0; j < 16; j++)
			{
				int col = (_vector[i][j]* 255) / _max;
				_rect.setPosition(sf::Vector2f(i*16,j*16));
				_rect.setFillColor(sf::Color(col,col,col));
				w.draw(_rect);
			}
		}
	}
};

class PathFinder
{
	sf::Vector2u _target;
	Vertex * _vertices[16][16];

public:
	std::vector<sf::Vector2u> getRoute(sf::Vector2u from, VectorField * v)
	{
		//start at from
		//follow vector field
		std::vector<sf::Vector2u> pts; pts.push_back(from);
		sf::Vector2u curP = from;
		while(curP != _target)
		{
			
			curP = v->getNextPoint(curP);
			std::cout << curP.x << " x " <<curP.y << " y " <<std::endl;
			pts.push_back(curP);
		}
		return pts;
	}
	void SetUpGrid(int size, BlockField * b)
	{	
			for (int i = 0; i < size; i++){for (int j = 0; j < size; j++)
			{
				if(!b->IsPointBlocked(sf::Vector2u(i,j)))
				{
					_vertices[i][j] = new Vertex(sf::Vector2u(i,j));
				}
				else
				{
					_vertices[i][j] = new Vertex(sf::Vector2u(i,j));
					_vertices[i][j]->set_blocked();
				}
			}
		}
		for (int i = 0; i < size; i++)
		{
			for (int j = 0; j < size; j++)
			{
				bool canLeft	= (i > 0		&& !_vertices[i-1][j]->is_blocked());
				bool canRight	= (i < size-1	&& !_vertices[i+1][j]->is_blocked());
				bool canUp		= (j > 0		&& !_vertices[i][j-1]->is_blocked());
				bool canDown	= (j < size-1	&& !_vertices[i][j+1]->is_blocked());

				if (canLeft){_vertices[i][j]->	AddPoint(_vertices[i-1][j],0);/*std::cout<< "+1 ";*/}else{_vertices[i][j]->set_next_to_blocked(0);}
				if (canRight){_vertices[i][j]->	AddPoint(_vertices[i+1][j],1);/*std::cout<< "+2 ";*/}else{_vertices[i][j]->set_next_to_blocked(1);}
				if (canUp)	{_vertices[i][j]->	AddPoint(_vertices[i][j-1],2);/*std::cout<< "+3 ";*/}else{_vertices[i][j]->set_next_to_blocked(2);}
				if (canDown){_vertices[i][j]->	AddPoint(_vertices[i][j+1],3);/*std::cout<< "+4 ";*/}else{_vertices[i][j]->set_next_to_blocked(3);}
			}
		}
	}

	VectorField get_vector_field(sf::Vector2u p)
	{
		_target = p;
		bool has_been_to[16][16];
		//int tile_dist[16][16];

		int max_count = 16*16;
		int current_count = 0;
		for (int i = 0; i < 16; i++)
		{
			for (int j = 0; j < 16; j++)
			{
				has_been_to[i][j] = false;
			}
		}
		_vertices[p.x][p.y]->get_tile_distance_from_point_recursive(has_been_to,0,&(current_count), max_count);
		VectorField vf;

		for (int i = 0; i < 16; i++)
		{
			for (int j = 0; j < 16; j++)
			{
				vf._vector[i][j] = _vertices[i][j]->get_distance_from_point();
				//std::cout << ":" <<_vertices[i][j]->get_distance_from_point();
			}
			//std::cout << std::endl;
		}
		
		vf.calc_max_value();

		return vf;
	}

};


class StateTestPathFiding 
{
	void run()
	{
		BlockField bf;
		for (int a = 0; a < 12; a++)
		bf.AddPoint(sf::Vector2u(8,a));

		for (int a = 8; a < 12; a++)
		bf.AddPoint(sf::Vector2u(a,12));

		for (int a = 12; a > 4; a--)
		bf.AddPoint(sf::Vector2u(12,a));

		PathFinder pf;
		pf.SetUpGrid(16,&bf);
		pf.get_vector_field(sf::Vector2u(2,8));

	}
};